#ifndef GMM_VOXEL_MAP_H
#define GMM_VOXEL_MAP_H


#include <iostream>
#include <Eigen/Core>
#include <gmm_voxel_map/gmm.h>
#include <vector>
#include <gmm_voxel_map/gmm_voxel_map_base.h>
#include <ros/ros.h>
#include <unordered_set>
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                
namespace gvm
{
class GMMVoxelMapCircularBuffer
{  
public:
	GMMVoxelMapCircularBuffer(){}
	~GMMVoxelMapCircularBuffer(){}

	GMMVoxelMapCircularBuffer(float resolution, float truncation_distance,int POW)
	:N(1<<POW),
	resolution_(resolution),
	truncation_distance_(truncation_distance),
	occupancy_buffer_(resolution,0.0,POW),
	flagNotAtedge_buffer((1<<POW) * (1<<POW)  * (1<<POW)),
	edt_buffer_(resolution,truncation_distance,POW),
	tmp_buffer1_(resolution,truncation_distance,POW),
	tmp_buffer2_(resolution,truncation_distance,POW)
	{
		trueProbability=0.95;  //p(z=1|s=1)=p(z=0|s=0)=trueProbability  
		falseProbability=1-trueProbability;  //p(z=0|s=1)=p(z=1|s=0)=falseProbability 
		lgHit=log(trueProbability/falseProbability);  //log(p(z=1|s=1)/p(z=1|s=0))
		lgMiss=log(falseProbability/trueProbability); //log(p(z=0|s=1)/p(z=0|s=0))

		occThreshold=3.476;  //97% occ probablity
		freeThreshold=-2.9;  //5%  occ probablity

		occlimit=occThreshold*1.5;
		freelimit=freeThreshold*1.5;

		std::fill(flagNotAtedge_buffer.begin(), flagNotAtedge_buffer.end(), false);

		// std::cout<<"lgHit,lgMiss,occThreshold,freeThreshold: "<<lgHit<<","<<lgMiss<<","<<occThreshold<<","<<freeThreshold<<std::endl; 

		updated_minIdx_<<9999,9999,9999;
		updated_maxIdx_<<-9999,-9999,-9999;

		std::cout<<"Map range: "<<N*resolution_<<"*"<<N*resolution_<<std::endl;



	}
	int N;
	double trueProbability;
	double falseProbability;

	double lgHit;
	double lgMiss;

	double occThreshold;
	double freeThreshold;

	double occlimit;
	double freelimit;

	float resolution_;
	float truncation_distance_;
	Eigen::Vector3d origin;

	VoxelMapCircularBufferBase<double> occupancy_buffer_;

	std::vector<bool> flagNotAtedge_buffer;

	VoxelMapCircularBufferBase<double> edt_buffer_;

	Eigen::Vector3i updated_minIdx_;   //Idx is local idx
	Eigen::Vector3i updated_maxIdx_;	//Idx is local idx
	VoxelMapCircularBufferBase<int> tmp_buffer1_;
	VoxelMapCircularBufferBase<int> tmp_buffer2_;

	std::unordered_set<int> recordedPoint;


public:

	inline double getMapGlobalPosEdt(Eigen::Vector3d pos) //pos is the global position
	{
		return edt_buffer_.getPosOccupiedProb(pos);
	}

	inline double getMapLocalIdxEdt(Eigen::Vector3i idx)
	{
		return edt_buffer_.getLocalIdxOccupiedProb(idx);
	}

	inline double getMapGlobalIdxEdt(Eigen::Vector3i idx)
	{
		return edt_buffer_.getGlobalIdxOccupiedProb(idx);
	}

	bool insideMapRange(Eigen::Vector3i idx)
	{
		return occupancy_buffer_.insideRange(idx);
	}

	inline void setMapInitPose(Eigen::Vector3d pos)
	{
		occupancy_buffer_.initPos=pos;
		occupancy_buffer_.mapCentrePos=pos;
	}

	inline int indexRange(){return occupancy_buffer_._N;}

	bool getMapGlobalIdxOccupied(Eigen::Vector3i idx)   //global index
	{
		double prob=occupancy_buffer_.getGlobalIdxOccupiedProb(idx);
		return prob>=occThreshold;
	}

	double getMapLocalIdxEntropy(Eigen::Vector3i idx)//idx is locbal index
	{
		double prob=occupancy_buffer_.getLocalIdxOccupiedProb(idx);
		prob=exp(prob)/(exp(prob)+1);

		return -prob*log(prob+0.001);

	}

	bool getMapLocalIdxOccupied(Eigen::Vector3i idx)   //idx is locbal index
	{
		double prob=occupancy_buffer_.getLocalIdxOccupiedProb(idx);
		return prob>=occThreshold;
	}

	bool getMapGlobalPosOccupied(Eigen::Vector3d pos)
	{
		double prob=occupancy_buffer_.getPosOccupiedProb(pos);
		return prob>=occThreshold;
	}

	Eigen::Vector3d getMapGlobalPosfromLocalIdx(Eigen::Vector3i idx)
	{
		return occupancy_buffer_.getGlobalPosfromLocalIdx(idx);
	}

	void getSurroundPts(const Eigen::Vector3d& pos, Eigen::Vector3d pts[2][2][2],Eigen::Vector3d& diff)  //pos is global pos 
	{
		/* interpolation position */
		Eigen::Vector3d pos_m = pos - 0.5 * resolution_* Eigen::Vector3d::Ones();
		Eigen::Vector3i idx;
		Eigen::Vector3d idx_pos;
		getmapIdxLocal(pos_m,idx);
		idx_pos=getMapGlobalPosfromLocalIdx(idx);
		diff = (pos - idx_pos) /resolution_;


		for (int x = 0; x < 2; x++) {
			for (int y = 0; y < 2; y++) {
			for (int z = 0; z < 2; z++) {
				Eigen::Vector3i current_idx = idx + Eigen::Vector3i(x, y, z);
				Eigen::Vector3d current_pos;
				current_pos=getMapGlobalPosfromLocalIdx(current_idx);
				pts[x][y][z] = current_pos;
			}
			}
		}	
	}

	void getSurroundDistance(Eigen::Vector3d pts[2][2][2], double dists[2][2][2]) 
	{
		for (int x = 0; x < 2; x++) {
			for (int y = 0; y < 2; y++) {
			for (int z = 0; z < 2; z++) {
				// ROS_INFO("pts[x][y][z]: %f %f %f", pts[x][y][z](0),pts[x][y][z](1),pts[x][y][z](2)); 
				dists[x][y][z] = getMapGlobalPosEdt(pts[x][y][z]);
			}
			}
		}
	}

	void interpolateTrilinear(double values[2][2][2],const Eigen::Vector3d& diff,double& value,Eigen::Vector3d& grad) 
	{
		// trilinear interpolation
		double v00 = (1 - diff(0)) * values[0][0][0] + diff(0) * values[1][0][0];
		double v01 = (1 - diff(0)) * values[0][0][1] + diff(0) * values[1][0][1];
		double v10 = (1 - diff(0)) * values[0][1][0] + diff(0) * values[1][1][0];
		double v11 = (1 - diff(0)) * values[0][1][1] + diff(0) * values[1][1][1];
		double v0 = (1 - diff(1)) * v00 + diff(1) * v10;
		double v1 = (1 - diff(1)) * v01 + diff(1) * v11;

		value = (1 - diff(2)) * v0 + diff(2) * v1;

		grad[2] = (v1 - v0)/resolution_;
		grad[1] = ((1 - diff[2]) * (v10 - v00) + diff[2] * (v11 - v01)) /resolution_;
		grad[0] = (1 - diff[2]) * (1 - diff[1]) * (values[1][0][0] - values[0][0][0]);
		grad[0] += (1 - diff[2]) * diff[1] * (values[1][1][0] - values[0][1][0]);
		grad[0] += diff[2] * (1 - diff[1]) * (values[1][0][1] - values[0][0][1]);
		grad[0] += diff[2] * diff[1] * (values[1][1][1] - values[0][1][1]);
		double resolution_inv_=1/resolution_;
		grad[0] *= resolution_inv_;
	}

	void evaluateEDTWithGrad(const Eigen::Vector3d& pos,double time, double& dist,Eigen::Vector3d& grad)   // get distance and gradient at pos, pos is a global position
	{
		Eigen::Vector3d diff;
		Eigen::Vector3d sur_pts[2][2][2];
		double dists[2][2][2];
		getSurroundPts(pos, sur_pts, diff);
		getSurroundDistance(sur_pts, dists);
		interpolateTrilinear(dists, diff, dist, grad);
	}	

	void updateOrigin(Eigen::Vector3d new_originPos)
	{
		occupancy_buffer_.updateMapOrigin(new_originPos);
		edt_buffer_.updateMapOrigin(new_originPos);

		updated_minIdx_<<9999,9999,9999;
		updated_maxIdx_<<-9999,-9999,-9999;

	}

	void MCRayTracing(std::vector<Eigen::Vector3d > samplePoints)
	{

		Eigen::Vector3i origin_index=occupancy_buffer_.mapCentreIdx;

		Eigen::Vector3i map_index;

		// ROS_INFO("origin_indexIDX: %d %d %d",origin_index(0),origin_index(1),origin_index(2));
		// ROS_INFO("samplePoints.SIZE: %ld",samplePoints.size());
		// ROS_INFO("gmm.n_component: %d",gmm.n_component_);

		for (std::vector<Eigen::Vector3d>::iterator iter = samplePoints.begin(); iter != samplePoints.end(); iter++)
		{
			getmapIdxLocal(*iter, map_index);

			// ROS_INFO("sample point: %f %f %f",(*iter)(0),(*iter)(1),(*iter)(2));
			// ROS_INFO("SAMPLE POINT IDX: %d %d %d",map_index(0),map_index(1),map_index(2));

			bool insideFlag=true;
			if(!insideMapRange(map_index))
			{
				insideFlag=false;
				map_index=occupancy_buffer_.getClosestIdxInRange(occupancy_buffer_.mapCentrePos,*iter);
			}

			if(map_index(0)>=indexRange()||map_index(1)>=indexRange()||map_index(2)>=indexRange()||map_index(0)<0||map_index(1)<0||map_index(2)<0)
			{
				ROS_ERROR("sample point out of range!!!!!!!!!");
				continue;
			}
			if(insideFlag)
				addHit(map_index);
			else
				addMiss(map_index);
			
			std::vector<Eigen::Vector3i> bresenhamPath;

			if(map_index==origin_index) continue;
			insertFreeBresenham3D(map_index, origin_index,bresenhamPath);

			for(std::vector<Eigen::Vector3i>::iterator iter_bresenhamPath= bresenhamPath.begin(); iter_bresenhamPath != bresenhamPath.end(); iter_bresenhamPath++)
			{
				// ROS_INFO("bressenham point: %d %d %d",(*iter_bresenhamPath)(0),(*iter_bresenhamPath)(1),(*iter_bresenhamPath)(2));
				addMiss(*iter_bresenhamPath);
			}

			// ROS_INFO("probability : %f",occupancy_buffer_.at(map_index));
		}
	}

	void MCRayTracing(GMM gmm, bool pointAtedge, double n_sigma) 
	{
		if(!pointAtedge) 
			recordedPoint.clear();
		std::vector<Eigen::Vector3d > samplePoints;
		int samplenum=1000;
		samplePoints=gmm.GMMRandomSamplePoints(samplenum,n_sigma);

		// for (int i=0;i<samplePoints.size();i++)
		// {
		// 	ROS_INFO("SAMPLE POINT: %f %f %f",samplePoints[i](0),samplePoints[i](1),samplePoints[i](2));
		// }

		Eigen::Vector3i origin_index;
		getmapIdxLocal(gmm.sensor_location_,origin_index);

		Eigen::Vector3i map_index;

		// ROS_INFO("origin_indexIDX: %d %d %d",origin_index(0),origin_index(1),origin_index(2));
		// ROS_INFO("samplePoints.SIZE: %ld",samplePoints.size());
		// ROS_INFO("gmm.n_component: %d",gmm.n_component_);

		for (std::vector<Eigen::Vector3d>::iterator iter = samplePoints.begin(); iter != samplePoints.end(); iter++)
		{
			getmapIdxLocal(*iter, map_index);
			if(!insideMapRange(map_index))
			{
				// ROS_INFO("!insideMapRange,map_index: %d %d %d",map_index(0),map_index(1),map_index(2));
				continue;
			}
			addHit(map_index);

			// // ROS_INFO("sample point: %f %f %f",(*iter)(0),(*iter)(1),(*iter)(2));
			// // ROS_INFO("SAMPLE POINT IDX: %d %d %d",map_index(0),map_index(1),map_index(2));

			// bool insideFlag=true;
			// // ROS_INFO("origin map index: %d %d %d",map_index(0),map_index(1),map_index(2));

			// if(!insideMapRange(map_index))
			// {
			// 	insideFlag=false;
			// 	map_index=occupancy_buffer_.getClosestIdxInRange(gmm.sensor_location_,*iter);
			// 	// ROS_INFO("cloesest map index: %d %d %d",map_index(0),map_index(1),map_index(2));
			// }

			// if(map_index(0)>=indexRange()||map_index(1)>=indexRange()||map_index(2)>=indexRange()||map_index(0)<0||map_index(1)<0||map_index(2)<0)
			// {
			// 	// ROS_ERROR("sample point out of range!!!!!!!!!");
			// 	continue;
			// }
			// if(insideFlag&&!pointAtedge)
			// {
			// 	addHit(map_index);
			// 	recordedPoint.insert(map_index(0)+map_index(1)*N+map_index(2)*N*N);
			// }
			// else
			// 	addMiss(map_index);
			
			// if(map_index==origin_index) continue;

			// std::vector<Eigen::Vector3i> bresenhamPath;
			// insertFreeBresenham3D(map_index, origin_index,bresenhamPath);

			// for(std::vector<Eigen::Vector3i>::iterator iter_bresenhamPath= bresenhamPath.begin(); iter_bresenhamPath != bresenhamPath.end(); iter_bresenhamPath++)
			// {
			// 	// ROS_INFO("bressenham point: %d %d %d",(*iter_bresenhamPath)(0),(*iter_bresenhamPath)(1),(*iter_bresenhamPath)(2));
			// 	if(pointAtedge&& recordedPoint.find((*iter_bresenhamPath)(0)+(*iter_bresenhamPath)(1)*N+(*iter_bresenhamPath)(2)*N*N)!=recordedPoint.end() )
			// 	{
			// 		break;
			// 	}
			// 	addMiss(*iter_bresenhamPath);
			// }

		}
	}


    template <typename F_get_val, typename F_set_val>
    void fill_edt(F_get_val f_get_val, F_set_val f_set_val, int start , int end ) 
    {
        int v[N];
        float z[N + 1];

        int k = start;
        v[start] = start;
        z[start] = -99999;
        z[start + 1] = 99999;

        for(int q = start + 1; q <= end; q++)
        {
            k++;
            float s;
            do
            {
                k--;
                s = ((f_get_val(q) + q * q) - (f_get_val(v[k]) + v[k] * v[k])) / (2 * q - 2 * v[k]);
                // ROS_INFO_STREAM("k: " << k << " s: " <<  s  << " z[k] " << z[k] << " v[k] " << v[k]);

            } while(s <= z[k]);

            k++;
            v[k] = q;
            z[k] = s;
            z[k + 1] = 99999;
        }

        k = start;

        for(int q = start; q <= end; q++)
        {
            while(z[k + 1] < q) k++;
            float val = (q - v[k]) * (q - v[k]) + f_get_val(v[k]);
            //      if(val < std::numeric_limits<_Scalar>::max())
            //  ROS_INFO_STREAM("val: " << val << " q: " << q << " v[k] " << v[k]);
            // if(val > truncation_distance_*truncation_distance_) val = std::numeric_limits<_Scalar>::max();
            f_set_val(q, val);
        }
    }


	void updateEDTMap()
	{
		if(updated_minIdx_(0)>updated_maxIdx_(0))  // no update voxel
			return;

		Eigen::Vector3i edtUpdate_minIdx=updated_minIdx_;
		Eigen::Vector3i edtUpdate_maxIdx=updated_maxIdx_;

        edtUpdate_minIdx.array() -= (truncation_distance_) / resolution_; 
		edtUpdate_maxIdx.array() += (truncation_distance_) / resolution_;

		edtUpdate_minIdx.array() = edtUpdate_minIdx.array().max(Eigen::Vector3i(0, 0, 0).array());  
        edtUpdate_maxIdx.array() = edtUpdate_maxIdx.array().min(Eigen::Vector3i(N - 1, N - 1, N - 1).array());

        updated_minIdx_.array() -= (truncation_distance_+0.1) / resolution_; 
		updated_maxIdx_.array() += (truncation_distance_+0.1) / resolution_; 

        updated_minIdx_.array() = updated_minIdx_.array().max(Eigen::Vector3i(0, 0, 0).array());  
        updated_maxIdx_.array() = updated_maxIdx_.array().min(Eigen::Vector3i(N - 1, N - 1, N - 1).array());

		// updated_minIdx_<<79,24,33;
		// updated_maxIdx_<<127,94,71;

		// ROS_INFO("updated_minIdx_: %d %d %d",updated_minIdx_(0),updated_minIdx_(1),updated_minIdx_(2));
		// ROS_INFO("updated_maxIdx_: %d %d %d",updated_maxIdx_(0),updated_maxIdx_(1),updated_maxIdx_(2));
		// ros::Time firstloopstart=ros::Time::now();
 		for(int x = updated_minIdx_(0); x <= updated_maxIdx_(0); x++)  //x, y plane search
        {
            for(int y = updated_minIdx_(1); y <= updated_maxIdx_(1); y++)
            {
                fill_edt([&](int z) {
                        return getMapLocalIdxOccupied( Eigen::Vector3i(x, y, z)) ?
                                   0 :
                                   9999; 
                    },
                    [&](int z, float val) { tmp_buffer1_.at(Eigen::Vector3i(x, y, z)) = val; }, updated_minIdx_(2), updated_maxIdx_(2));
            }
        }		
		// ros::Time firstloopend=ros::Time::now();
		// ROS_INFO("FIRST TIME USE: %f",(firstloopend-firstloopstart).toSec());

 		for(int x = updated_minIdx_(0); x <= updated_maxIdx_(0); x++)   // x, z plane search
        {
            for(int z = updated_minIdx_(2); z <= updated_maxIdx_(2); z++)
            {
                fill_edt([&](int y) { return tmp_buffer1_.at(Eigen::Vector3i(x, y, z)); },
                         [&](int y, float val) { tmp_buffer2_.at(Eigen::Vector3i(x, y, z)) = val; }, updated_minIdx_(1), updated_maxIdx_(1));
            }
        }

        for(int y = updated_minIdx_(1); y <= updated_maxIdx_(1); y++) // y, z plane search
        {
            for(int z = updated_minIdx_(2); z <= updated_maxIdx_(2); z++)
            {
                fill_edt([&](int x) { return tmp_buffer2_.at(Eigen::Vector3i(x, y, z)); },
                         [&](int x, float val) 
						 {
							 Eigen::Vector3i tmp(x,y,z);
							 if(x>=edtUpdate_minIdx(0) && x<=edtUpdate_maxIdx(0) &&
							 	 y>=edtUpdate_minIdx(1) && y<=edtUpdate_maxIdx(1) &&
								  z>=edtUpdate_minIdx(2) && z<=edtUpdate_maxIdx(2))
                             edt_buffer_.at(Eigen::Vector3i(x, y, z)) =std::min(resolution_ * std::sqrt(val), truncation_distance_);  // Set final offset
                         },
                         updated_minIdx_(0), updated_maxIdx_(0));
            }
        }

		updated_minIdx_<<9999,9999,9999;
		updated_maxIdx_<<-9999,-9999,-9999;

	}


    Eigen::Vector3i getLocalIdxMapEdtColor(Eigen::Vector3i idx)  //idx is local map idx
    {
        // double distance = getMapLocalIdxEdt(idx);
		// Eigen::Vector3i point_RGB;
		// if(distance>=0.8)
		// {
		// 	point_RGB(0) = 0;
		// 	point_RGB(1) = 0;
		// 	point_RGB(2) = 255;
		// }
		// else{
		// 	point_RGB(0) = 255;
		// 	point_RGB(1) = 0;
		// 	point_RGB(2) = 0;
		// }
        // return point_RGB;

        // Eigen::Vector3i point_RGB;

		// point_RGB(0)=255-distance/truncation_distance_*255;
		// point_RGB(1)=0;
		// point_RGB(2)=distance/truncation_distance_*255;
		// return point_RGB;


        double distance = getMapLocalIdxEdt(idx);
        int value = floor(distance * 240); // Mapping 0~1.0 to 0~240
        value = value > 240 ? 240 : value;
		int interval=60;
        // 240 degrees are divided into 4 sections, 0 is Red-dominant, 1 and 2 are Green-dominant,
        // 3 is Blue-dominant. The dorminant color is 255 and another color is always 0 while the
        // remaining color increases or decreases between 0~255
        int section = value / interval;
        float float_key = (value % interval) / (float)interval * 255;
        int key = floor(float_key);
        int nkey = 255 - key;
        Eigen::Vector3i point_RGB;
        switch(section) {
            case 0: // G increase
                point_RGB(0) = 255;
                point_RGB(1) = key;
                point_RGB(2) = 0;
                break;
            case 1: // R decrease
                point_RGB(0) = nkey;
                point_RGB(1) = 255;
                point_RGB(2) = 0;
                break;
            case 2: // B increase
                point_RGB(0) = 0;
                point_RGB(1) = 255;
                point_RGB(2) = key;
                break;
            case 3: // G decrease
                point_RGB(0) = 0;
                point_RGB(1) = nkey;
                point_RGB(2) = 255;
                break;
            case 4:
                point_RGB(0) = 0;
                point_RGB(1) = 0;
                point_RGB(2) = 255;
                break;
            default: // White
                point_RGB(0) = 255;
                point_RGB(1) = 255;
                point_RGB(2) = 255;
        }
        return point_RGB;
    }



	void getmapIdxGlobal(Eigen::Vector3d coordinate,Eigen::Vector3i & index)
	{
		occupancy_buffer_.getIdxGlobal(coordinate, index);  
  	}

	void getmapIdxLocal(Eigen::Vector3d coordinate,Eigen::Vector3i & index)  // get a local idx form global coordinate ,coordinate is global position, index is local idx ranging from 0~N-1
	{
		occupancy_buffer_.getIdxLocal(coordinate, index);  
  	}

	void addHit(Eigen::Vector3i index)
	{
		bool last_occupied= getMapLocalIdxOccupied(index);

		occupancy_buffer_.at(index)=occupancy_buffer_.at(index)+lgHit;
		if(occupancy_buffer_.at(index)>=occlimit)
			occupancy_buffer_.at(index)=occlimit;

		bool new_occupied= getMapLocalIdxOccupied(index);
		if(last_occupied!=new_occupied)
		{
			updated_minIdx_ = updated_minIdx_.array().min(index.array());
			updated_maxIdx_ = updated_maxIdx_.array().max(index.array());
		}
	}

	void addMiss(Eigen::Vector3i index)
	{
		bool last_occupied= getMapLocalIdxOccupied(index);

		occupancy_buffer_.at(index)=occupancy_buffer_.at(index)+lgMiss;
		if(occupancy_buffer_.at(index)<=freelimit)
			occupancy_buffer_.at(index)=freelimit;

		bool new_occupied= getMapLocalIdxOccupied(index);
		if(last_occupied!=new_occupied)
		{
			updated_minIdx_ = updated_minIdx_.array().min(index.array());
			updated_maxIdx_ = updated_maxIdx_.array().max(index.array());
		}		
	}

	void insertFreeBresenham3D(const Eigen::Vector3i &point_idx,
								const Eigen::Vector3i &origin_idx,std::vector<Eigen::Vector3i>& bresenhamPath) 
	{

		bresenhamPath.push_back(origin_idx);

		int dx = abs(point_idx(0) - origin_idx(0) );
		int dy =abs(point_idx(1) - origin_idx(1) );
		int dz =abs(point_idx(2) - origin_idx(2) );
		int xs,ys,zs;
		xs=(point_idx(0)> origin_idx(0))?1:-1;
		ys=(point_idx(1)> origin_idx(1))?1:-1;
		zs=(point_idx(2)> origin_idx(2))?1:-1;

		int x1,y1,z1,x2,y2,z2;
		x1=origin_idx(0);    y1=origin_idx(1);    z1=origin_idx(2);
		x2=point_idx(0);    y2=point_idx(1);    z2=point_idx(2);

		int p1,p2;
		// # Driving axis is X-axis"
		if (dx >= dy && dx >= dz)
		{
			p1 = 2 * dy - dx;
			p2 = 2 * dz - dx;
			while (x1+xs != x2)
			{
				x1 += xs;
				if (p1 >= 0)
				{
					y1 += ys;
					p1 -= 2 * dx;
				}

				if (p2 >= 0)
				{
					z1 += zs;
					p2 -= 2 * dx;
				}

				p1 += 2 * dy;
				p2 += 2 * dz;
				
				Eigen::Vector3i tmp(x1,y1,z1);
				bresenhamPath.push_back(tmp);
			}

		}

		// # Driving axis is Y-axis"
		else if (dy >= dx and dy >= dz)
		{
			p1 = 2 * dx - dy;
			p2 = 2 * dz - dy;
			while (y1+ys != y2)
			{
				y1 += ys;
				if (p1 >= 0)
				{
					x1 += xs;
					p1 -= 2 * dy;
				}
				if (p2 >= 0)
				{
					z1 += zs;
					p2 -= 2 * dy;
				}
				p1 += 2 * dx;
				p2 += 2 * dz;
				Eigen::Vector3i tmp(x1,y1,z1);
				bresenhamPath.push_back(tmp);        
			}

		}

		// # Driving axis is Z-axis"
		else{
			p1 = 2 * dy - dz;
			p2 = 2 * dx - dz;
			while (z1 +zs!= z2)
			{
				z1 += zs;
				if (p1 >= 0)
				{
					y1 += ys;
					p1 -= 2 * dz;
				}
				if (p2 >= 0)
				{
					x1 += xs;
					p2 -= 2 * dz;
				}
				p1 += 2 * dy;
				p2 += 2 * dx;
				Eigen::Vector3i tmp(x1,y1,z1);
				bresenhamPath.push_back(tmp); 
		
			}
		}
	}

};
}

#endif
